#include "RunToPositionBehaviour.h"
#include "../CommonFunctions.h"

/*
 * Bezi na zadanu poziciu, na ktorej ocakava najvhodnejsie zachytenie lopty
 * 
 * @param position	miesto kam sa ma bezat
 * @param speedMode	0-2 velkost zrychlenia
 */
void RunToPositionBehaviour::Behave(Point position, int speedMode)
{
	// Nema zmysel bezat na poziciu, ak uz tam som
/*	if((position - world->me->pos).GetLength() < world->me->type->kickable_margin)
	{
		TurnTo(world->ball->pos + world->ball->vel);
		return;
	}
*/
	float dashpower;

	switch(speedMode)
	{
	case 0:
		dashpower = RecommDashPower(false);
		break;
	case 1:
		dashpower = RecommDashPower(true);
		break;
	case 2:
		dashpower = RecommDashPower(true, true);
		break;
	default:
		dashpower = RecommDashPower(true);
		break;
	}

	// Bod urcite leziaci na ihrisku
	Point correctPlaygroundPoint = position;

	if(correctPlaygroundPoint.GetX() <= -52.5) correctPlaygroundPoint.SetX(-51);
	if(correctPlaygroundPoint.GetX() >= 52.5) correctPlaygroundPoint.SetX(51);
	if(correctPlaygroundPoint.GetY() <= -34) correctPlaygroundPoint.SetY(-33);
	if(correctPlaygroundPoint.GetY() >=  34) correctPlaygroundPoint.SetY(33);

	Vector diff = correctPlaygroundPoint - world->me->pos;
	Angle uhol = CalcDir(diff, world->me->GetAbsDir());
	
	Ball tmpBall(*(world)->ball);	
	tmpBall.Step();

	if(diff.GetLength() > 0)
	{
		if(uhol.GetAbsDeg() > 10)
		{
			float turnMoment = CompMoment(uhol.GetDeg());
			commandGenerator->Turn(turnMoment);
			GetTurnNeckToBehaviour()->Behave(tmpBall.pos, world->me->pos + world->me->vel, 
				GetTurnToBehaviour()->CompTurn(turnMoment));
		}
		else
		{
			commandGenerator->Dash(dashpower);
			GetTurnNeckToBehaviour()->Behave(tmpBall.pos, world->me->pos +  
				PredictNextVelocity(dashpower), 0);
		}
	}
}

/*
 * bezi sa do bodu s istym natocenim
 *
 * @param target cielovy bod kam sa bezi
 * @param hurry ci sa ma ponahlat
 * @param forLife ci ma bezat ako o zivot :)
 * @param positionTolerance tolerancia vzialenosti od urceneho bodu
 * @param angleTolerance tolerancia natocenia hraca
 */
void RunToPositionBehaviour::Behave(const Point& target, bool hurry, bool forLife, float positionTolerance, int angleTolerance)
{	
	float power;
			
	if(forLife)
		power = RecommendDashToMaxSpeed();
	else 
		power = RecommDashPower(hurry);

	// Kde zastaneme ?
	Point stopPos = world->me->pos + PredictNextVelocity(power);
	Vector diff = target - stopPos;

	Ball tmpBall(*world->ball);
	tmpBall.Step();

	if(diff.GetLength() > positionTolerance)
	{
		// Sme prilis daleko
//		float angle = CalcDir(target - world->me->pos, world->me->GetAbsDir());

//		Point futurePos = world->me->pos + PredictNextVel(power);
//		float turnMoment = CompMoment(CalcDir(target - futurePos, world->me->GetAbsDir()).GetDeg());

		float turnMoment = CompMoment(CalcDir(target - world->me->pos, world->me->GetAbsDir()).GetDeg());

		if(fabsf(turnMoment) > angleTolerance)
		{
			// Musime sa otocit
			commandGenerator->Turn(turnMoment);

			GetTurnNeckToBehaviour()->Behave(tmpBall.pos, world->me->pos + world->me->vel, 
				GetTurnToBehaviour()->CompTurn(turnMoment));
		}
		else
		{
			// Nemusime sa otocit
			commandGenerator->Dash(power);
			GetTurnNeckToBehaviour()->Behave(tmpBall.pos, world->me->pos + PredictNextVelocity(power), 0);
		}
	}
}

/*
 * Bez na zadnu poziciu celom vpred (pre brankara)
 *
 * @param target cielovy bod kam sa bezi
 * @param hurry ci sa ma ponahlat
 * @param forLife ci ma bezat ako o zivot :)
 * @param positionTolerance tolerancia vzialenosti od urceneho bodu
 * @param angleTolerance tolerancia natocenia hraca
 *
 */
void RunToPositionBehaviour::BehaveBackwards(const Point& target, bool hurry, bool forLife, float positionTolerance, int angleTolerance)
{
	float power;
			
	if(forLife)
		power = RecommendDashToMaxSpeed();
	else 
		power = RecommDashPower(hurry);

	// Kde zastaneme?
	Point stopPos = world->me->pos + PredictNextVelocity(power);
	Vector diff = target - stopPos;

	Ball tmpBall(*world->ball);
	tmpBall.Step();

	if(diff.GetLength() > positionTolerance)
	{
		// Sme prilis daleko
		float angle = CalcDir(target - world->me->pos, world->me->GetAbsDir()).GetDeg();

		if(fabsf(angle) < 180 - angleTolerance)
		{
			// Musime sa otocit
//			Point futurePos = world->me->pos + PredictNextVel(power);
//			float turnMoment = CompMoment(CalcDir(RectVector(0, 0) - (target - futurePos), world->me->GetAbsDir()).GetDeg());
			
			float turnMoment = CompMoment(CalcDir(RectVector(0, 0) - (target - world->me->pos), world->me->GetAbsDir()).GetDeg());
			
			commandGenerator->Turn(turnMoment);

			GetTurnNeckToBehaviour()->Behave(tmpBall.pos, world->me->pos + world->me->vel,
				GetTurnToBehaviour()->CompTurn(turnMoment));
		}
		else
		{
			// Nemusime sa otocit
			float k=.5f;
			if(hurry)
			{
				k=.75f;
				if(forLife)
					k=1.0f;
			}
			commandGenerator->Dash(-power * k);
			GetTurnNeckToBehaviour()->Behave(tmpBall.pos, world->me->pos + PredictNextVelocity(power), 0);
		}
	}
}


/*
 * Vypocita doporuceny maximalny koeficient sprintu
 *
 * @param fast	ci ma hrac bezat rychlo
 * @param reallyFast	ak je to mozne, zrychli hraca o 50%
 *
 * @return	vypocitany koeficient sprintu
 */
float RunToPositionBehaviour::RecommDashPower(bool fast, bool reallyFast)
{
	const float power_low = min((world->me->type->stamina_inc_max) * world->params->server->recover_min, 
		world->params->server->maxpower);
	const float stamina_low = world->params->server->stamina_max * world->params->server->effort_dec_thr;
	
	if(world->me->GetEffort() < world->me->type->effort_max)
		return power_low; // je unaveny
	if(world->me->GetStamina() <= stamina_low)
		return power_low; // pod 30%
	
	const float stamina_mid = world->params->server->stamina_max * world->params->server->effort_inc_thr;
	const float slow_powermax = min(world->me->type->stamina_inc_max, world->params->server->maxpower);
	
    float result;
    
	if(fast && world->state->GetPlayMode() == PM_PlayOn) // bezime rychlo
	{
		if(world->me->GetStamina() < (stamina_mid)) // pod 60%
		{
			result = slow_powermax + (world->me->GetStamina() - stamina_low) * 
				(world->params->server->maxpower - slow_powermax)/(stamina_mid - stamina_low);
		}
		else // nad 60%
		{
			result = world->params->server->maxpower;
		}
	}
	else // bezime pomaly
	{
/*		if(world->me->GetStamina() < stamina_mid) // pod 60%
		{
			result = power_low;
		}
		else // nad 60%
		{
*/			
			if(world->me->GetStamina() < world->params->server->stamina_max) // pod 100%
			{
				result = power_low + (world->me->GetStamina() - stamina_mid) * 
					(slow_powermax - power_low)/(world->params->server->stamina_max - stamina_mid);
				result *= 1.75f;
			}
			else // stamina je plna
			{
				result = slow_powermax;
			}
//		}
	}

	if (reallyFast)
	{
        result *= 1.5f;
		if (result > 100.0f)
			result = 100.0f;
    }

	float MaxNeededDash = RecommendDashToMaxSpeed();
/*	DBGN("\n%d", world->state->GetSimTime());
	DBGN("\n\n\n\nRealDash %3.2f", result);
	DBGN("\n\tToMaxSpeed: %3.2f", MaxNeededDash);
*/
	if (result > MaxNeededDash)
		result = MaxNeededDash;

    return result;
}

/*
 * Vypocita zrychlenie na dosiahnutie maximalnej rychlosti
 *
 * @return	vypocitane zrychlenie
 */
float RunToPositionBehaviour::RecommendDashToMaxSpeed()
{
	float RealSpeedMax = CalcRealSpeedMax();
	float neededAcceleration = RealSpeedMax - world->me->vel.GetLength();
//	DBGN("Needed accel: %f", neededAcceleration);
	float power = neededAcceleration / (world->me->effort * 
		world->params->server->dash_power_rate);
	
	if (power > world->params->server->maxpower)
		power = world->params->server->maxpower;
	return power;
}

/*
 *	Vypocita najvyssiu dosiahnutelnu rychlost hraca
 *
 * @return	najvyssia dosiahnutelna rychlost hraca
 */
float RunToPositionBehaviour::CalcRealSpeedMax()
{
	PLAYER_TYPE* player_type = world->me->type;
	float max_accel = 100.0f * player_type->dash_power_rate * player_type->effort_max;
    float real_speed_max = max_accel / (1.0f - player_type->player_decay);
    if ( real_speed_max > player_type->player_speed_max )
    {
        real_speed_max = player_type->player_speed_max;
    }
	return real_speed_max;
}

/*
 *	Kompenzovanie momentu zotrvacnosti
 *
 *	@param	moment	velkost momentu, ktory treba kompenzovat
 *
 *	@return	hodnota, ktorou sa kompenzuje zotrvacnost
 */
float RunToPositionBehaviour::CompMoment(float moment)
{
	float returnedValue = moment * (1 + world->me->vel.GetLength() * world->me->type->inertia_moment);
	if(returnedValue > world->params->server->maxmoment)
	{
		return world->params->server->maxmoment;
	}
	else if(returnedValue < world->params->server->minmoment)
	{
		return world->params->server->minmoment;
	}
	return returnedValue;
}

/*
 * Funkcia na predikovanie stavu hraca po prikaze dash
 *
 * @param me odkaz na mna (hraca)
 * @param moment sila zrychlenia, po ktorom sa ma predikovat
 */
Myself RunToPositionBehaviour::AfterDash(const Myself& me,float dashPower)
{
	//zmeni sa pozicia, vektor rychlosti
	Myself tmpMe(me);
	tmpMe.vel = PredictNextVelocity(dashPower,tmpMe);
	tmpMe.pos = tmpMe.pos+tmpMe.vel;
	tmpMe.vel *= tmpMe.type->player_decay;
	return tmpMe;
}

/*
 *	Predikuje rychlost v dalsom kroku. Vola metodu vytvorenu Loptosmi...
 *
 *	@param	dashPower	velkost zrychlenia
 *
 *	@return	vektor udavajuci smer a rychlost?
 */
Vector RunToPositionBehaviour::PredictNextVelocity(float dashPower)
{
	return PredictNextVelocity(dashPower,*(world)->me);
}


/*
 *	Predikuje rychlost v dalsom kroku. Vylepsena metoda Loptosov
 *
 *	@param	dashPower	velkost zrychlenia
 *	@param	me	informacie o mne	
 *
 *	@return	vektor udavajuci smer a rychlost?
 */
Vector RunToPositionBehaviour::PredictNextVelocity(float dashPower, const Myself &me)
{
	if(dashPower > world->params->server->maxpower)
		dashPower = world->params->server->maxpower;
	if(dashPower < world->params->server->minpower)
		dashPower = world->params->server->minpower;

	float ep = dashPower * me.type->dash_power_rate * me.GetEffort(); //co ma byt ep netusim, estimatedPower?
	
	if(ep > world->params->server->player_accel_max)
		ep = world->params->server->player_accel_max;
	if(ep < -world->params->server->player_accel_max)
		ep = -world->params->server->player_accel_max;

	float uhol = me.GetAbsDir().GetRad();

	RectVector nextVel(me.vel.GetX() + ep * cosf(uhol),me.vel.GetY() + ep * sinf(uhol));

	if(nextVel.GetLength() > me.type->player_speed_max)
		nextVel *= (me.type->player_speed_max / nextVel.GetLength());

	return nextVel;
}


/* Vypocita kolko krokov by trvalo prekonat danu vzdialenost so zadanym koeficientom sprintu a rychlosti
 *
 *	@param distance	vzdialenost, ktoru treba prekonat
 *	@param	power	koeficient sprintu
*	@param	actualSpeed	aktualna rychlost
*
*	@return	pocet krokov potrebnych na prekonanie vzdialenosti 
 */
int RunToPositionBehaviour::PredictSteps(float distance, float power, float actualSpeed)
{
	int steps = 0;
	const float acc = power * world->me->GetEffort() * world->me->type->dash_power_rate;
	while(distance > 0)
	{
		actualSpeed = min(world->me->type->player_speed_max, actualSpeed + acc);
		distance -= actualSpeed;
		actualSpeed *= world->me->type->player_decay;
		steps++;
	}

	return steps;
}

/*
 * Hrac odbehne z ofsajdu
 */
void RunToPositionBehaviour::ExitOffside()
{
	//RunToPoint(Point((GetOffsideLineBreachBehaviour()->GetOffsideLine() - 1.0f), world->me->pos.GetY()), true);
	Behave(Point((DATA_STORAGE.GetOffsideLine() - 1.0f), world->me->pos.GetY()), true);
}

/*
 * Najde lepsiu poziciu pre obdrzanie nahravky a presunie tam hraca.
 *
 * @return vrati sa true, ak sa hrac presunul, false, ak sa nenaslo ziadne lepsie miesto
 */
bool RunToPositionBehaviour::GotoPositionToPass()
{
	Vector positions[10];
	positions[0].SetX(5.0f);
	positions[0].SetY(0.0f);
	positions[1].SetX(3.5f);
	positions[1].SetY(3.5f);
	positions[2].SetX(3.5f);
	positions[2].SetY(-3.5f);
	positions[3].SetX(0.0f);
	positions[3].SetY(5.0f);
	positions[4].SetX(0.0f);
	positions[4].SetY(-5.0f);

	int i;
	for (i = 0; i < 5; i++)
	{
		positions[i + 5].SetX(positions[i].GetX() * 2);
		positions[i + 5].SetY(positions[i].GetY() * 2);
	}

	Point fpDefaultPosition = DATA_STORAGE.GetHomePos();
	float fMyEnemyDistance = DATA_STORAGE.GetNearestEnemyDistance(world->me->pos);
	// Pre kazdu z pozicii zistim, ci tam hrac moze ist
	for (i = 0; i < 10; i++)
	{
		Point fpDestination = world->me->pos + positions[i];
		
		if (IsOffsidePosition(fpDestination) == true)
			continue;
		
		float fTargetDistance = (fpDestination - fpDefaultPosition).GetLength();
		float fMyDistance = (world->me->pos - fpDefaultPosition).GetLength();
		
		if (fTargetDistance < 20.0f || fTargetDistance < fMyDistance)
		{
			// Teoreticky tam mozem ist, este zistit, ci sa to oplati...
			float fTargetEnemyDistance = DATA_STORAGE.GetNearestEnemyDistance(fpDestination);
			if (fTargetEnemyDistance > (fMyEnemyDistance + 2.0f) && 
				DATA_STORAGE.GetNearestFriendDistance(fpDestination) > 5.0f)
			{
				//RunToPoint(fpDestination, true);
				Behave(fpDestination, true);
				return true;
			}
		}
	}
	
	return false;
}

/*
 * Zisti ci je urcene miesto v ofsaje
 *
 * @param target bod, ktory chceme overit
 *
 * @return ci je bod v ofsajde
 */
bool RunToPositionBehaviour::IsOffsidePosition(Point target)
{
	const float off_l = DATA_STORAGE.GetOffsideLine();

	if(target.GetX() >= off_l && world->me->pos.GetX() > off_l - 1.0f)
		return true;

	return false;
}
